#include "registration_impl.h"

namespace Registration {

RegistrationImpl::RegistrationImpl() {
  dox_ = new Dox;
  point_cloud_count = 0;
}

RegistrationImpl::~RegistrationImpl() { delete dox_; }

void RegistrationImpl::SetCameraParams(
    Registration::CameraParams &camera_params_) {
  camera_params.focal_x = camera_params_.focal_x;
  camera_params.focal_y = camera_params_.focal_y;
  camera_params.image_width = camera_params_.image_width;
  camera_params.image_height = camera_params_.image_height;
  camera_params.principal_x = camera_params_.principal_x;
  camera_params.principal_y = camera_params_.principal_y;
}

void RegistrationImpl::InitRegistration() {
  coarse_registration.Init(dox_);
  coarse_registration.GenerateDescriptor();
  coarse_registration.MatchFpfhDescriptor();
}

void RegistrationImpl::SetInputSourcePointCloud(
    pcl::PointCloud<pcl::PointXYZ> &input_point_cloud) {
  dox_->current_dense_point_cloud = input_point_cloud;
  point_cloud_count++;
}

// compute point cloud normal
void RegistrationImpl::ComputePointCloudNormal() {
  /*normal estimation*/
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation;
  pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
      new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud(dox_->current_sparse_point_cloud.makeShared());
  // save kdtree for descriptor generator
  //  dox_->current_kdtree = tree;
  normal_estimation.setInputCloud(
      dox_->current_sparse_point_cloud.makeShared());
  normal_estimation.setSearchMethod(tree);
  normal_estimation.setKSearch(20);
  normal_estimation.compute(*normals);
  /*get normal direction*/
  GetNormalDirection(normals);
  // save normal for descriptor generator
  dox_->current_normal = normals;
  // connect normal and pointcloud
  pcl::concatenateFields(dox_->current_sparse_point_cloud, *normals,
                         dox_->current_sparse_point_cloud_with_normal);
}

void RegistrationImpl::Reset() {}

void RegistrationImpl::IcpRegistration() {}

void RegistrationImpl::Run() {

  // downsample point cloud
  DownsamplePointCloud();
  // compute point cloud normal
  ComputePointCloudNormal();
  // fpfh registration,in this step descriptor generator and point match
  InitRegistration();
  // icp registration
  // IcpRegistration();
}

void RegistrationImpl::GetNormalDirection(
    pcl::PointCloud<pcl::Normal>::Ptr &normals) {
  Eigen::Vector3f target_normal(0, 0, -1);

  for (int i = 0; i < normals->size(); i++) {
    pcl::Normal &normal = normals->points[i];
    Eigen::Vector3f temp_normal_vector(normal.normal_x, normal.normal_y,
                                       normal.normal_z);
    if (target_normal.dot(temp_normal_vector) < 0) {
      normal.normal_z = -normal.normal_z;
      normal.normal_y = -normal.normal_y;
      normal.normal_x = -normal.normal_x;
    }
  }
}
// down sample point cloud
void RegistrationImpl::DownsamplePointCloud() {
  pcl::VoxelGrid<pcl::PointXYZ> sor;
  sor.setInputCloud(dox_->current_dense_point_cloud.makeShared());
  sor.setLeafSize(0.1, 0.1, 0.1);
  sor.filter(dox_->current_sparse_point_cloud);
}

} // namespace Registration